/*
 * @Author: Mcfly
 * @Date: 2021-07-27 11:23:57
 * @LastEditTime: 2021-11-05 20:23:46
 * @LastEditors: Mcfly
 * @Description: 
 * @FilePath: \FTTfly\Ctrl\XY_Ctrl.c
 * 本人github:https://github.com/McflyWZX
 */
#include "XY_Ctrl.h"
#include "ti_system.h"
#include "PID.h"
#include "stdlib.h"
#include "math.h"
#include "ButterWorth.h"
#include "LimitAmpFilter.h"


ButterParameter *butterWorthFlowXY = NULL;
ButterBufferData butterWorthFlowX_Buf = {0.0f, 0.0f, 0.0f};
ButterBufferData butterWorthFlowY_Buf = {0.0f, 0.0f, 0.0f};

ButterParameter *butterWorthXYd = NULL;
ButterBufferData butterWorthXYd_Buf[2] = {{0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}};

LimitAmpParameter *LimitAmpXYout = NULL;

XY_Ctrl *XY_Ctrl_Init(void)
{
	butterWorthFlowXY = ButterWorth_Init(66.0f, 8.0f);
    butterWorthXYd = ButterWorth_Init(66.0, 20);
	LimitAmpXYout = LimitAmpFilter_Init(0.2f, 0.2f);
    XY_Ctrl *xyCtrl = (XY_Ctrl *)malloc(sizeof(XY_Ctrl));
	while(!butterWorthFlowXY);
    while(!butterWorthXYd);
	while(!LimitAmpXYout);
	while(!xyCtrl);
	
    //MV：xyCtrl->PID_X_Pos = *initPID(0.05, 0.092, 0.0824, 13.334, 1 / 30.0f);
	xyCtrl->PID_X_Pos = *initPID(0.45f, 0, 0, 20, 1 / 66.0f);
    // xyCtrl->PID_X_Speed = *initPID(0.75f, 0.1f, 0, 13, 1 / 95.97850081581726f);
	xyCtrl->PID_X_Speed = *initPID(1.811978f, 2.375f, 0.2172f, 32, 1 / 95.97850081581726f);//d0.0217035f
    //MV：xyCtrl->PID_Y_Pos = *initPID(0.04, 0.068667f, 0.06815385f, 20, 1 / 30.0f);
	xyCtrl->PID_Y_Pos = *initPID(0.45f, 0, 0, 20, 1 / 66.0f);
    // xyCtrl->PID_Y_Speed = *initPID(0.75f, 0.1f, 0, 13, 1 / 95.97850081581726f);
	xyCtrl->PID_Y_Speed = *initPID(1.811978f, 2.375f, 0.2172f, 32, 1 / 95.97850081581726f);
	
	xyCtrl->OptPos.x = 0;
	xyCtrl->OptPos.y = 0;
	XY_Ctrl_Clear(xyCtrl);
    return xyCtrl;
}

void XY_Ctrl_SpeedUpdateFrame_Single(XY_Ctrl *xyCtrl, Vector2f nowXY_Speed, Vector2f expectXY_Speed, Vector2f nowAccel, Vector2f nowGyro, Height *height)
{

	
	if(fabs(nowXY_Speed.x) > 100)
		nowXY_Speed.x = xyCtrl->lastXY_Speed.x;
	if(fabs(nowXY_Speed.y) > 100)
		nowXY_Speed.y = xyCtrl->lastXY_Speed.y;
	
	if(fabs(nowXY_Speed.x) < 0.01)
		nowXY_Speed.x = 0;
	if(fabs(nowXY_Speed.y) < 0.01)
		nowXY_Speed.y = 0;

	xyCtrl->lastXY_Speed.x = ButterWorth_Work_LPB(nowXY_Speed.x, &butterWorthFlowX_Buf, butterWorthFlowXY) * (height->h + 0.2f) * 0.75f;// - nowAngle.x;
	xyCtrl->lastXY_Speed.y = ButterWorth_Work_LPB(nowXY_Speed.y, &butterWorthFlowY_Buf, butterWorthFlowXY) * (height->h + 0.2f) * 0.75f;// - nowAngle.y;
	xyCtrl->OptPos.x += xyCtrl->lastXY_Speed.x / 66.0f;
	xyCtrl->OptPos.y += xyCtrl->lastXY_Speed.y / 66.0f;

//	nowAccel.x = 0.95f * nowAccel.x * 85.0f + 0.05f * (xyCtrl->lastXY_Speed.x - xyCtrl->lastXY_Pos.x) * 66.0f;
//	nowAccel.y = -0.95f * nowAccel.y * 85.0f + 0.05f * (xyCtrl->lastXY_Speed.y - xyCtrl->lastXY_Pos.y) * 66.0f;
	
	xyCtrl->lastXY_AccelSpeed.x += 9.0f * nowAccel.x / 66.0f;
	xyCtrl->lastXY_AccelSpeed.y += - 9.0f * nowAccel.y / 66.0f;
	xyCtrl->lastXY_AccelSpeed.x = 0.94f * xyCtrl->lastXY_AccelSpeed.x + 0.06f * xyCtrl->lastXY_Speed.x;
	xyCtrl->lastXY_AccelSpeed.y = 0.94f * xyCtrl->lastXY_AccelSpeed.y + 0.06f * xyCtrl->lastXY_Speed.y;
	xyCtrl->lastXY_AccelPos.x += 9.0f * nowAccel.x / 66.0f;
	xyCtrl->lastXY_AccelPos.y += - 9.0f * nowAccel.y / 66.0f;

	//float expectSpeedX = PIDProcessWithDiffTriErr(&xyCtrl->PID_X_Speed, expectXY_Speed.x - xyCtrl->lastXY_AccelSpeed.x, expectXY_Speed.x - xyCtrl->lastXY_Speed.x, expectXY_Speed.x - xyCtrl->lastXY_AccelPos.x);
	float expectSpeedX = PIDProcessWithSelfIntegralErrAndFilterD(&xyCtrl->PID_X_Speed, expectXY_Speed.x - (xyCtrl->lastXY_AccelSpeed.x - nowGyro.x), expectXY_Speed.x - xyCtrl->lastXY_Speed.x, &butterWorthXYd_Buf[0], butterWorthXYd);
    //float expectSpeedY = PIDProcessWithDiffTriErr(&xyCtrl->PID_Y_Speed, expectXY_Speed.y - xyCtrl->lastXY_AccelSpeed.y, expectXY_Speed.y - xyCtrl->lastXY_Speed.y, expectXY_Speed.y - xyCtrl->lastXY_AccelPos.y);
	float expectSpeedY = PIDProcessWithSelfIntegralErrAndFilterD(&xyCtrl->PID_Y_Speed, expectXY_Speed.y - (xyCtrl->lastXY_AccelSpeed.y - nowGyro.y), expectXY_Speed.y - xyCtrl->lastXY_Speed.y, &butterWorthXYd_Buf[1], butterWorthXYd);

	xyCtrl->outPut.x = -LIMIT(-8.0f, 8.0f, expectSpeedX);
	xyCtrl->outPut.y = -LIMIT(-8.0f, 8.0f, expectSpeedY);
	// xyCtrl->outPut.x = ButterWorth_Work_LPB(xyCtrl->outPut.x, &butterWorthFlowXout_Buf, butterWorthFlowXYout);
	// xyCtrl->outPut.y = ButterWorth_Work_LPB(xyCtrl->outPut.y, &butterWorthFlowYout_Buf, butterWorthFlowXYout);
	
	//xyCtrl->outPut.x = dx;
	//xyCtrl->outPut.y = dy;
}

void XY_Ctrl_PosUpdateFrame(XY_Ctrl *xyCtrl, Vector2f nowXY_Pos, Vector2f expectXY_Pos)
{
	float expectSpeedX = PIDProcess(&xyCtrl->PID_X_Pos, expectXY_Pos.x - xyCtrl->OptPos.x);
	float expectSpeedY = PIDProcess(&xyCtrl->PID_Y_Pos, expectXY_Pos.y - xyCtrl->OptPos.y);

	xyCtrl->outPut.x = LIMIT(-8.0f, 8.0f, expectSpeedX);
	xyCtrl->outPut.y = LIMIT(-8.0f, 8.0f, expectSpeedY);
}

void XY_Ctrl_Clear(XY_Ctrl *xyCtrl)
{
    xyCtrl->PID_X_Pos.integral = 0;
	xyCtrl->PID_X_Speed.integral = 0;
    xyCtrl->PID_Y_Pos.integral = 0;
	xyCtrl->PID_Y_Speed.integral = 0;
    xyCtrl->lastXY_Speed.x = 0;
    xyCtrl->lastXY_Speed.y = 0;
	// xyCtrl->lastXY_Pos.x = 0;
	// xyCtrl->lastXY_Pos.y = 0;
	xyCtrl->lastXY_AccelSpeed.x = 0;
	xyCtrl->lastXY_AccelSpeed.y = 0;
	xyCtrl->lastXY_AccelPos.x = 0;
	xyCtrl->lastXY_AccelPos.y = 0;
    xyCtrl->outPut.x = 0;
    xyCtrl->outPut.y = 0;
}
